home *** CD-ROM | disk | FTP | other *** search
/ Resource for Source: C/C++ / Resource for Source - C-C++.iso / codelib9 / v_11_03 / 1103028a < prev    next >
Encoding:
Text File  |  1995-11-01  |  20.4 KB  |  948 lines
  1. /****************************************************/
  2. /*                                                  */
  3. /*      SSX.C - stack swap executive                */
  4. /*                                                  */
  5. /*      By Tom Green and Dennis Cronin              */
  6. /*      10/19/92                                    */
  7. /*                                                  */
  8. /****************************************************/
  9.  
  10. /* turn on inline asm */
  11. #pragma inline
  12.  
  13. #include <alloc.h>
  14. #include <dos.h>
  15. #include <string.h>
  16. #include <setjmp.h>
  17. #include "ssx.h"
  18. #include "ssx_conf.h"
  19.  
  20. /*  Task Control Block */
  21.  
  22. typedef struct tcb {
  23.     /* task chain forward ptr */
  24.     struct tcb *forw;
  25.     /* task chain backward ptr */
  26.     struct tcb *back;
  27.     /* delay chain forward ptr */
  28.     struct tcb *dforw;
  29.     /* delay chain backward ptr */
  30.     struct tcb *dback;
  31.     /* pointer to task code */
  32.     fptr task_ptr;
  33.     /* pointer to start of allocated stack */
  34.     unsigned int *stack;
  35.     /* task's current stack pointer */
  36.     unsigned int *stack_ptr;
  37.     /* delay counter */
  38.     long timeout;
  39.     /* flag for task timed out */
  40.     unsigned char timedout;
  41.     /* flag for TCB in use */
  42.     unsigned char active;
  43.     /* status flags */
  44.     unsigned char status;
  45.     /* task priority */
  46.     unsigned char priority;
  47.     /* task ID */
  48.     unsigned char id;
  49.     /* for storing extra task context */
  50.     char context[CNTXT_SZ];
  51. } tcb;
  52.  
  53.  
  54. /* misc. defines */
  55. #define TRUE            1
  56. #define FALSE           0
  57.  
  58. /* background task defines */
  59. #define BG_TASK_ID      0xff
  60. #define BG_TASK_PRI     0xff
  61.  
  62. /*  make data and code local to this file */
  63. #define LOCAL static
  64.  
  65. /* flags for the TCB status word */
  66. #define T_READY         0    /* ready to run */
  67. #define T_WAITING       1    /* waiting on wait_q */
  68. #define T_DELAYED       2    /* delay timer running */
  69.  
  70.  
  71. /* local function prototypes */
  72.  
  73. LOCAL tcb *get_tcb(void);
  74. LOCAL void free_tcb(tcb *tbp);
  75. LOCAL void put_ready(tcb *tbp);
  76. LOCAL void rotate_tasks(tcb *tbp);
  77. LOCAL void put_delay(long timeout);
  78. LOCAL void run_new_task(void);
  79. LOCAL void bg_task(void);
  80. LOCAL void stack_swap(unsigned int **old_stack_ptr,
  81.                         unsigned int **new_stack_ptr);
  82. LOCAL int disable_ints(void);
  83.  
  84.  
  85. /*  local variables  */
  86.  
  87. LOCAL long sys_time;                /* system timer */
  88. LOCAL unsigned char slice_cnt;
  89. LOCAL int running;
  90. LOCAL int initd;
  91. LOCAL jmp_buf jbuf;
  92. LOCAL int switch_lock;
  93.  
  94. /* task control */
  95. LOCAL tcb t_pool[MAX_TASKS + 1]; /* pool of TCBs */
  96. LOCAL tcb t_ready;    /* head of ready task queue */
  97. LOCAL tcb t_null;     /* the NULL task */
  98. LOCAL tcb *t_free;    /* head of free queue */
  99. LOCAL tcb *t_current; /* pointer to current task */
  100.  
  101. /* delay control */
  102. LOCAL tcb d_chain;    /* q head for delayed tasks */
  103.  
  104. /* MACROS to unlink from task and delay queues */
  105.  
  106. /* t_unlink - must be used w/ interrupts off */
  107. #define     t_unlink(tbp)                           \
  108.      {                                              \
  109.         (tbp)->back->forw = (tbp)->forw;            \
  110.         if((tbp)->forw != NULL)                     \
  111.             (tbp)->forw->back = (tbp)->back;        \
  112.     }
  113.  
  114. /* d_unlink - must be used w/ interrupts off */
  115. #define     d_unlink(tbp)                           \
  116.     {                                               \
  117.         (tbp)->dback->dforw = (tbp)->dforw;         \
  118.         if((tbp)->dforw != NULL)                    \
  119.             (tbp)->dforw->dback = (tbp)->dback;     \
  120.     }
  121.  
  122.  
  123. /*
  124.  * ssx_init - init ssx data
  125.  */
  126.  
  127. int
  128. ssx_init(void)
  129. {
  130.     int i;
  131.     tcb *tcbp;
  132.  
  133.     if(initd)
  134.         return(INIT_ERROR);
  135.  
  136.     memset(&d_chain,0,sizeof(d_chain));
  137.  
  138.     /* init TCB free queue links */
  139.     for(i=0,tcbp=t_pool;i < MAX_TASKS-1;i++,tcbp++){
  140.         tcbp->forw = &t_pool[i+1];
  141.     }
  142.     t_pool[i].forw = NULL;
  143.  
  144.     for(i = 0;i < MAX_TASKS;i++){
  145.         t_pool[i].active=FALSE;
  146.     }
  147.  
  148.     t_current = NULL;
  149.     t_free = t_pool;
  150.     t_ready.forw = NULL;
  151.     switch_lock = 0;
  152.  
  153.     /* set up background task */
  154.     if((ssx_task_create(BG_TASK_PRI,BG_TASK_ID,
  155.                         bg_task,0x200,"bg_task"))
  156.                         != SUCCESS)
  157.         return(INIT_ERROR);
  158.  
  159.     initd = TRUE;
  160.  
  161.     return(SUCCESS);
  162. }
  163.  
  164.  
  165. /*
  166.  * sx_run - this starts executive
  167.  */
  168.  
  169. void
  170. ssx_run(void)
  171. {
  172.     int val;
  173.  
  174.     val = setjmp(jbuf);
  175.     
  176.     if(val != 0)
  177.         return;
  178.  
  179.     slice_cnt = 0;
  180.     sys_time = 0;
  181.  
  182.     /* make current task ptr point to dummy tcb so
  183.      * beginning of time stack pointer save will
  184.      * have a safe place to save to.
  185.      */
  186.  
  187.     t_current = &t_null;
  188.  
  189.     /* mark SSX as active */
  190.     running = TRUE;
  191.     
  192.     /* this will start the first task rolling */
  193.     ssx_switch();
  194. }
  195.  
  196. /*
  197.  * sx_stop - this stops executive
  198.  */
  199.  
  200. void
  201. ssx_stop(void)
  202. {
  203.     int i;
  204.     int_state_var istate;
  205.  
  206.     ints_off(istate);
  207.  
  208.     /* free any allocated stacks */
  209.     for(i = 0; i < MAX_TASKS; i++){
  210.         if(t_pool[i].stack != NULL){
  211.             free(t_pool[i].stack);
  212.             t_pool[i].stack = NULL;
  213.         }
  214.     }
  215.     initd = FALSE;
  216.     running = FALSE;
  217.     restore_ints(istate);
  218.  
  219.     longjmp(jbuf,1);
  220. }
  221.  
  222.  
  223. /*
  224.  * ssx_task_create - create task and set up tcb
  225.  */
  226.  
  227. int
  228. ssx_task_create(unsigned char task_pri,
  229.                 unsigned char task_id,fptr task_ptr,
  230.                 unsigned int stack_size,char *name)
  231. {
  232.     unsigned int i;
  233.     tcb *tbp;
  234.     int_state_var istate;
  235.  
  236.     ints_off(istate);
  237.  
  238.     if(task_id == 0) {
  239.         restore_ints(istate);
  240.         return(TID_ERR);
  241.     }
  242.  
  243.     /* check for TID already in use */
  244.     for(i = 0,tbp = t_pool;i < MAX_TASKS;i++,tbp++) {
  245.         if(tbp->active && tbp->id == task_id) {
  246.             restore_ints(istate);
  247.             return(TID_ERR);
  248.         }
  249.     }
  250.  
  251.     if((tbp = get_tcb()) == NULL) {    /* get a tcb */
  252.         restore_ints(istate);
  253.         return(TCB_ERR);
  254.     }
  255.  
  256.  
  257.     /* allocate stack for this task */
  258.     if((tbp->stack = (unsigned int *)
  259.                       malloc(stack_size)) == NULL){
  260.         restore_ints(istate);
  261.         return(STACK_ERR);
  262.     }
  263.  
  264.     /* fill in the blanks */
  265.     strncpy(tbp->context,name,CNTXT_SZ);
  266.     tbp->priority = task_pri;
  267.     tbp->id = task_id;
  268.     tbp->status = T_READY;
  269.     tbp->timedout = FALSE;
  270.     tbp->timeout = 0L;
  271.     tbp->forw = tbp->back =
  272.     tbp->dforw = tbp->dback = NULL;
  273.  
  274.     tbp->task_ptr = task_ptr;
  275.  
  276.     tbp->stack_ptr = (unsigned int *)(tbp->stack +
  277.                      (stack_size / 2));
  278.  
  279.     /* setup task stack to have address of start up
  280.      * routine and fake di, si, bp registers to pop.
  281.      * This part is not portable. the stack looks 
  282.      * like this:
  283.      *
  284.      *  |-------------------------|         high
  285.      *  |address of run_new_task  |
  286.      *  |-------------------------|
  287.      *  |bp                       |
  288.      *  |-------------------------|
  289.      *  |si                       |
  290.      *  |-------------------------|
  291.      *  |di                       |
  292.      *  |-------------------------|         low
  293.      *
  294.      */
  295.  
  296.     *(--tbp->stack_ptr) = (unsigned int)run_new_task;
  297.     *(--tbp->stack_ptr) = 0;     /* fake BP,SI,DI */
  298.     *(--tbp->stack_ptr) = 0;     /* on stack */
  299.     *(--tbp->stack_ptr) = 0;
  300.  
  301.     /* put on active chain */
  302.     rotate_tasks(tbp);
  303.  
  304.     ssx_switch();
  305.  
  306.     restore_ints(istate);
  307.  
  308.     return(SUCCESS);
  309. }
  310.  
  311.  
  312. /*
  313.  * ssx_task_delay - cause task to delay for number
  314.  *                  of ticks
  315.  */
  316.  
  317. void
  318. ssx_task_delay(long timeout)
  319. {
  320.     int_state_var istate;
  321.     
  322.     ints_off(istate);
  323.  
  324.     if(timeout == 0) {
  325.         ssx_switch();
  326.         restore_ints(istate);
  327.         return;
  328.     }
  329.  
  330.     put_delay(timeout);  /* put current task on */
  331.                          /* delay queue */
  332.     t_unlink(t_current); /* take off ready queue */
  333.  
  334.     ssx_switch();
  335.     restore_ints(istate);
  336. }
  337.  
  338.  
  339. /*
  340.  * ssx_task_delete - delete a task and remove from
  341.  *                   queues
  342.  */
  343.  
  344. int
  345. ssx_task_delete(unsigned char task_id)
  346. {
  347.     unsigned int i;
  348.     tcb *tp;
  349.     int_state_var istate;
  350.  
  351.     ints_off(istate);
  352.  
  353.     /* look for background task ID */
  354.     if(task_id == BG_TASK_ID){
  355.         restore_ints(istate);
  356.         return(TID_ERR);
  357.     }
  358.  
  359.     /* look for 'self' form */
  360.     if(task_id == 0) {
  361.         if(t_current) {
  362.             /* get current task's id */
  363.             task_id = t_current->id;
  364.         }
  365.     }
  366.  
  367.     /* brute force, search all TCBs for matching ID */
  368.     for(i = 0,tp = t_pool;i < MAX_TASKS;i++,tp++)   {
  369.         if(tp->active && tp->id == task_id) {
  370.             break;
  371.         }
  372.     }
  373.  
  374.  
  375.     /* see if found match */
  376.     if(i == MAX_TASKS){
  377.         restore_ints(istate);
  378.         return(TID_ERR);
  379.     }
  380.  
  381.     switch(tp->status & (T_DELAYED | T_WAITING)) {
  382.         case T_DELAYED:
  383.             d_unlink(tp); /* remove from delay q */
  384.             break;
  385.         case T_WAITING:
  386.             t_unlink(tp); /* remove from some */
  387.                           /* wait_q */
  388.             break;
  389.         case T_DELAYED | T_WAITING:
  390.             t_unlink(tp); /* remove from some */
  391.                           /* wait_q */
  392.             d_unlink(tp); /* remove from delay q */
  393.             break;
  394.         case T_READY:
  395.             t_unlink(tp); /* remove from ready q */
  396.             break;
  397.     }
  398.  
  399.     free(tp->stack);     /* free allocated stack */
  400.     tp->stack = NULL;
  401.     free_tcb(tp);        /* free up the TCB */
  402.     ssx_switch();
  403.     restore_ints(istate);
  404.     return(SUCCESS);
  405. }
  406.  
  407.  
  408.  
  409. /*
  410.  * ssx_change_priority - change priority for currently
  411.  *                       running task,
  412.  *                       don't immediately reschedule
  413.  *                       returns old priority
  414.  */
  415.  
  416. unsigned char
  417. ssx_change_priority(unsigned char new_priority)
  418. {
  419.     unsigned char old_priority;
  420.     int_state_var istate;
  421.  
  422.     ints_off(istate);
  423.     old_priority = t_current->priority;
  424.     t_current->priority = new_priority;
  425.     t_unlink(t_current);
  426.     rotate_tasks(t_current);
  427.     restore_ints(istate);
  428.     return(old_priority);
  429. }
  430.  
  431.  
  432. /*
  433.  * ssx_wait - wait on wait_q. reschedule later
  434.  */
  435.  
  436. void
  437. ssx_wait(wait_q *wqptr)
  438. {
  439.     tcb *tp;
  440.     tcb *t_cur;
  441.     int_state_var istate;
  442.  
  443.     ints_off(istate);
  444.  
  445.     /* check for message flag already set */
  446.     if(wqptr->mesg_flg){
  447.         wqptr->mesg_flg = FALSE;
  448.         restore_ints(istate);
  449.         return;
  450.     }
  451.  
  452.     t_cur = t_current;
  453.     t_unlink(t_cur);  /* take off ready queue */
  454.  
  455.     tp = (tcb *)&wqptr->task_ptr;
  456.  
  457.     /*
  458.      * find where to insert waiting task into
  459.      * wait queue
  460.      */
  461.     while((tp->forw) != NULL) {
  462.         if(t_cur->priority <= tp->forw->priority)
  463.             break;
  464.         tp = tp->forw;
  465.     }
  466.  
  467.     /* insert into queue */
  468.     if((t_cur->forw = tp->forw) != NULL)
  469.         t_cur->forw->back = t_cur;
  470.     tp->forw = t_cur;
  471.     t_cur->back = tp;
  472.     t_cur->status = T_WAITING;
  473.  
  474.     ssx_switch();
  475.     restore_ints(istate);
  476. }
  477.  
  478.  
  479. /* 
  480.  * ssx_wait_with alarm - wait on wait_q with alarm.
  481.  *                       reschedule now
  482.  */
  483.  
  484. int
  485. ssx_wait_with_alarm(wait_q *wqptr,long timeout)
  486. {
  487.     tcb *tp;
  488.     tcb *t_cur;
  489.     int_state_var istate;
  490.  
  491.     ints_off(istate);
  492.  
  493.     /* check for message flag already set */
  494.     if(wqptr->mesg_flg){
  495.         wqptr->mesg_flg = FALSE;
  496.         restore_ints(istate);
  497.         return(SUCCESS);
  498.     }
  499.  
  500.     t_cur = t_current;
  501.     t_unlink(t_cur);  /* take off ready queue */
  502.  
  503.     tp = (tcb *)&wqptr->task_ptr;
  504.  
  505.     /*
  506.      * find where to insert waiting task into
  507.      * wait queue
  508.      */
  509.     while((tp->forw) != NULL) {
  510.         if(t_cur->priority <= tp->forw->priority)
  511.             break;
  512.         tp = tp->forw;
  513.     }
  514.  
  515.     /* insert into queue */
  516.     if((t_cur->forw = tp->forw) != NULL)
  517.         t_cur->forw->back = t_cur;
  518.     tp->forw = t_cur;
  519.     t_cur->back = tp;
  520.     t_cur->status = T_WAITING;
  521.  
  522.     /*
  523.      * if there is timeout value, put task on
  524.      * delay queue
  525.      */
  526.     if(timeout)
  527.         put_delay(timeout);
  528.  
  529.     ssx_switch();
  530.  
  531.     /* 
  532.      * we were sheduled back in so
  533.      * see if task timed out and return error
  534.      */
  535.  
  536.     if(t_cur->timedout){
  537.         t_cur->timedout = FALSE;
  538.         restore_ints(istate);
  539.         return(TO_ERR);
  540.     }
  541.  
  542.     restore_ints(istate);
  543.  
  544.     /* task did not time out, so return success */
  545.     return(SUCCESS);
  546. }
  547.  
  548.  
  549. /* 
  550.  * ssx_alert - alert wait_q. reshedule if task
  551.  *             alerted is equal or higher
  552.  *             priority than current task
  553.  */
  554.  
  555. int
  556. ssx_alert(wait_q *wqptr)
  557. {
  558.     tcb *np;
  559.     tcb *oldtcb;
  560.     int_state_var istate;
  561.  
  562.     ints_off(istate);
  563.  
  564.     /* check for message waiting */
  565.     if(wqptr->mesg_flg){
  566.         restore_ints(istate); /* cannot alert if */
  567.         return(MW_ERR);       /* messgae is waiting */
  568.     }
  569.  
  570.     np=(tcb *)wqptr->task_ptr;
  571.  
  572.     /* check if there is a task waiting on wait_q */
  573.     if(np != NULL){
  574.         t_unlink(np);
  575.         if(np->status & T_DELAYED)
  576.             d_unlink(np);
  577.         np->status &= ~(T_WAITING | T_DELAYED);
  578.         put_ready(np);
  579.         /*
  580.          * switch to waiting task if it is equal
  581.          * or higher priority
  582.          */
  583.         if(np->priority <= t_current->priority){
  584.             oldtcb = t_current;
  585.             t_current = np;
  586.             /*
  587.              * check and see if scheduling is
  588.              * disabled
  589.              */
  590.             if(switch_lock == 0)
  591.                 stack_swap(&oldtcb->stack_ptr
  592.                           ,&np->stack_ptr);
  593.         }
  594.         restore_ints(istate);
  595.         return(SUCCESS);
  596.     }
  597.  
  598.     /* fell thru, simply leave message in wait_q */
  599.     wqptr->mesg_flg = TRUE;
  600.     restore_ints(istate);
  601.     return(SUCCESS);
  602. }
  603.  
  604.  
  605. /*
  606.  * ssx_clock_tick - call this to update ssx clock from
  607.  *                  timer interrupt handler
  608.  */
  609.  
  610. void
  611. ssx_clock_tick(void)
  612. {
  613.     tcb *tp;
  614.     int_state_var istate;
  615.  
  616.     ints_off(istate);
  617.  
  618.     if(running == FALSE){
  619.         restore_ints(istate);
  620.         return;
  621.     }
  622.  
  623.     sys_time++;
  624.  
  625.     /* do time updates */
  626.     tp = (tcb *)&d_chain;
  627.     /* check for timed out tasks */
  628.     while((tp = tp->dforw) != NULL) {
  629.         if((sys_time - tp->timeout) >= 0) {
  630.             d_unlink(tp);   /* this one's ready */
  631.             tp->timedout = TRUE;
  632.             if(tp->status & T_WAITING)
  633.                 t_unlink(tp);
  634.             tp->status = T_READY;
  635.             /* put task on ready queue */
  636.             rotate_tasks(tp);
  637.         }
  638.         else
  639.             break;         /* passed the ready ones */
  640.     }
  641.  
  642.     /*  round robin rotation */
  643.     if((++slice_cnt) == TIME_SLICE){
  644.         slice_cnt = 0;
  645.         /* if task is running and was left ready */
  646.         if(t_current && t_current->status
  647.             == T_READY) {
  648.             /* remove from ready queue */
  649.             t_unlink(t_current);
  650.             /* puts at back of pri group */
  651.             rotate_tasks(t_current);
  652.         }
  653.     }
  654.  
  655.     ssx_switch();
  656.     restore_ints(istate);
  657. }
  658.  
  659.  
  660. /*
  661.  * ssx_set_time - this sets SSX system time
  662.  */
  663.  
  664. void
  665. ssx_set_time(long time)
  666. {
  667.     int_state_var istate;
  668.  
  669.     ints_off(istate);
  670.     sys_time = time;
  671.     restore_ints(istate);
  672. }
  673.  
  674.  
  675. /*
  676.  * ssx_get_time - this returns SSX system time
  677.  */
  678.  
  679. long
  680. ssx_get_time(void)
  681. {
  682.     return(sys_time);
  683. }
  684.  
  685.  
  686. /*
  687.  * ssx_lock - disable task switching
  688.  */
  689.  
  690. void
  691. ssx_lock(void)
  692. {
  693.     int_state_var istate;
  694.  
  695.     ints_off(istate);
  696.     switch_lock++;
  697.     restore_ints(istate);
  698. }
  699.  
  700. /*
  701.  * ssx_unlock - enable task switching
  702.  */
  703.  
  704. void
  705. ssx_unlock(void)
  706. {
  707.     int_state_var istate;
  708.  
  709.     ints_off(istate);
  710.     /* call ssx_switch if we are not nested */
  711.     if(--switch_lock == 0)
  712.         ssx_switch();
  713.     restore_ints(istate);
  714. }
  715.  
  716. /*
  717.  * ssx_switch - run next ready task
  718.  *
  719.  * notes: there must always be a runnable task w/
  720.  * SSX. a background task is created by ssx_init
  721.  * and this task can never wait or do anything
  722.  * that would remove it from the active queue.
  723.  * this saves checks in this routine, making
  724.  * it more efficient.
  725.  */
  726.  
  727. void
  728. ssx_switch(void)
  729. {
  730.     tcb *oldtcb;
  731.  
  732.     if(!running)
  733.         return;
  734.  
  735.     oldtcb = t_current;
  736.  
  737.     /* switch tasks */
  738.     t_current = t_ready.forw; /* get next */
  739.                               /* ready task */
  740.     /*
  741.      * if new task is same as old, do not
  742.      * bother with switch
  743.      */
  744.     if(t_current == oldtcb)
  745.         return;
  746.  
  747.     /* check and see if scheduling is disabled */
  748.     if(switch_lock == 0){
  749.         /* we have a new task so do a task switch */
  750.         stack_swap(&oldtcb->stack_ptr,
  751.                    &t_current->stack_ptr);
  752.     }
  753. }
  754.  
  755.  
  756.  
  757. /*
  758.  * get_tcb - get task control block
  759.  */
  760.  
  761. LOCAL tcb *
  762. get_tcb(void)
  763. {
  764.     tcb *tbp;
  765.  
  766.     if(t_free == NULL)
  767.         return(NULL);
  768.     tbp = t_free;
  769.     t_free = tbp->forw;
  770.     tbp->active = TRUE;
  771.  
  772.     return(tbp);
  773. }
  774.  
  775.  
  776. /*
  777.  * free_tcb - free task control block
  778.  */
  779.  
  780. LOCAL void
  781. free_tcb(tcb *tbp)
  782. {
  783.     /* '****' for debug TCB not in use */
  784.     tbp->timeout = 0x2a2a2a2aL;
  785.     tbp->active = FALSE;
  786.     tbp->forw = t_free;
  787.     t_free = tbp;
  788. }
  789.  
  790.  
  791. /*
  792.  * put_ready - put task at head of ready queue
  793.  */
  794.  
  795. LOCAL void
  796. put_ready(tcb *tbp)
  797. {
  798.     tcb *tp,*np;
  799.     unsigned int priority;
  800.  
  801.     /* get priority of task to be inserted in chain */
  802.     priority = tbp->priority;
  803.  
  804.     /* put on the active chain */
  805.     tp = (tcb *)&t_ready;
  806.     /*
  807.      * sort in order of decreasing priority, put at
  808.      * head of pri group
  809.      */
  810.     while((np = tp->forw) != NULL && np->priority
  811.                                      < priority)
  812.         tp = np;
  813.     /* link in */
  814.     tbp->forw = np;
  815.     tbp->back = tp;
  816.     tp->forw = tbp;
  817.     if(np != NULL)
  818.         np->back = tbp;
  819. }
  820.  
  821. /*
  822.  * rotate_tasks - put task at back of ready queue
  823.  */
  824.  
  825. LOCAL void
  826. rotate_tasks(tcb *tbp)
  827. {
  828.     tcb *tp,*np;
  829.     unsigned int priority;
  830.  
  831.     /* get priority of task to be inserted in chain */
  832.     priority = tbp->priority;
  833.  
  834.     /* put on the active chain */
  835.     tp = (tcb *)&t_ready;
  836.     /*
  837.      * sort in order of decreasing priority,
  838.      * put at back of pri group
  839.      */
  840.     while((np = tp->forw) != NULL && np->priority
  841.                                      <= priority)
  842.         tp = np;
  843.     /* link in */
  844.     tbp->forw = np;
  845.     tbp->back = tp;
  846.     tp->forw = tbp;
  847.     if(np != NULL)
  848.         np->back = tbp;
  849. }
  850.  
  851. /*
  852.  * put_delay - put task on delay queue
  853.  */
  854.  
  855. LOCAL void
  856. put_delay(long timeout)
  857. {
  858.     tcb *tp,*np;
  859.  
  860.     t_current->timeout =
  861.         timeout + sys_time;   /* actual time ready */
  862.     t_current->timedout=FALSE;
  863.     t_current->status |= T_DELAYED;
  864.     tp = (tcb *)&d_chain;
  865.     /* sort in order increasing target time */
  866.     /* trick to solve wrap of sys_time */
  867.     while((np = tp->dforw) != NULL) {
  868.         if(timeout <= np->timeout - sys_time)
  869.             /* hit a more future one */
  870.             break;
  871.         tp = np;
  872.     }
  873.     /* link in */
  874.     t_current->dforw = np;
  875.     t_current->dback = tp;
  876.     tp->dforw = t_current;
  877.     if(np != NULL)
  878.         np->dback = t_current;
  879. }
  880.  
  881.  
  882. /*
  883.  * run_new_task - starts up a new task making sure
  884.  *                interrupts are enabled
  885.  */
  886.  
  887. LOCAL void
  888. run_new_task(void)
  889. {
  890.     ints_on();
  891.     (t_current->task_ptr)();
  892. }
  893.  
  894. /*
  895.  * bg_task - must have a background task
  896.  */
  897.  
  898. LOCAL void
  899. bg_task(void)
  900. {
  901.     while(1);
  902. }
  903.  
  904.  
  905. /*
  906.  * WARNING - routines from here on are not portable
  907.  */
  908.  
  909. /*
  910.  * stack_swap - switch from stack of curent task to
  911.  *              stack of new task
  912.  */
  913.  
  914. LOCAL void
  915. stack_swap(unsigned int **old_stack_ptr,
  916.            unsigned int **new_stack_ptr)
  917. {
  918.     asm or  di,di   /* fool compiler into saving */ 
  919.     asm or  si,si   /* di and si registers */
  920.  
  921.     /* save stack pointer of old task from sp reg */
  922.     *old_stack_ptr = (unsigned int *)_SP;
  923.  
  924.     /* load sp reg with stack pointer of new task */
  925.     _SP=(unsigned int)*new_stack_ptr;
  926. }
  927.  
  928.  
  929. /*
  930.  * disable_ints - disable interrupts and return state
  931.  *                of interrupts before before they
  932.  *                were disabled. Returns positive
  933.  *                integer if they were enabled
  934.  */
  935.  
  936. LOCAL int
  937. disable_ints(void)
  938. {
  939.     asm pushf           /* save flags to get */
  940.                         /* interupt status */
  941.     asm cli             /* interrupts off */
  942.     asm pop ax          /* get interrupt state from */
  943.                         /* flags that were pushed */
  944.     asm and ax,0200h    /* and flags to get interrupt */
  945.                         /* status */
  946.     return(_AX);
  947. }
  948.